package simpleGeom;

import java.awt.geom.Line2D;

import processing.core.PMatrix3D;
import processing.core.PVector;

public class Triangle3DUVN extends Face3D {
	
	Vec3D u;
	Vec3D v;
	Vec3D n;
	float d0;
	
	
	
	public Triangle3DUVN(HasPos3D p0,HasPos3D p1, HasPos3D p2){
		this.addNode(p0);
		this.addNode(p1);
		this.addNode(p2);
		init();
	}
	public void init(){
	     u=Vec3D.sub(getNode(1), getNode(0));
	     v=Vec3D.sub(getNode(2), getNode(0));
	     n=Vec3D.cross(u,v);
	     n.normalize();
	     d0=-n.dot(getNode(0));
	     
	}
	double SMALL_NUM = 0.0000000001; // anything that avoids division overflow
	// dot product (3D) which allows vector operations in arguments
	
	// intersect_RayTriangle(): intersect a ray with a 3D triangle
//	    Input:  a ray R, and a triangle T
//	    Output: *I = intersection point (when it exists)
//	    Return: -1 = triangle is degenerate (a segment or point)
//	             0 = disjoint (no intersect)
//	             1 = intersect in unique point I1
//	             2 = are in the same plane
	
	
	
	public Vec3D intersect_RayTriangle( HasPos3D r0,HasPos3D r1 )
	{
	     // triangle vectors
	    Vec3D i=rayPlaneIntersection(r0,r1,false);
	    if (i==null)return null;
	    // is I inside T?
	    double    uu, uv, vv, wu, wv, D;
	    uu = Vec3D.dot3D(u,u);
	    uv = Vec3D.dot3D(u,v);
	    vv = Vec3D.dot3D(v,v);
	    Vec3D w =Vec3D.sub( i,getNode(0)) ;
	    wu = Vec3D.dot3D(w,u);
	    wv =Vec3D.dot3D(w,v);
	    D = uv * uv - uu * vv;
	    if (Math.abs(D)<SMALL_NUM)return null;
	    // get and test parametric coords
	    double s, t;
	    s = (uv * wv - vv * wu) / D;
	    if (s < 0.0 || s > 1.0)        // I is outside T
	        return null;
	    t = (uv * wu - uu * wv) / D;
	    if (t < 0.0 || (s + t) > 1.0)  // I is outside T
	        return null;

	    return i;                      // I is in T
	}
	public Vec3D intersect_RayTriangle2( HasPos3D r0,HasPos3D r1 )
	{
	     // triangle vectors
	    Vec3D i=rayPlaneIntersection(r0,r1,false);
	    if (i==null)return null;
	    // is I inside T?
	    HasPos3D p1=this.getNode(0);
	    HasPos3D p2=this.getNode(1);
	    HasPos3D p3=this.getNode(2);
	   // sameNormal
	    if (Line2D.relativeCCW( p1.getX(), p1.getY(),  p2.getX(), p2.getY(),p3.getX(), p3.getY())==1){
	    	HasPos3D t=p1;
	    	p1=p3;
	    	p3=t;
	    }
	   if (Line2D.relativeCCW( p1.getX(), p1.getY(),  p2.getX(), p2.getY(),r0.getX(), r0.getY())==1)return null;
	   if (Line2D.relativeCCW( p2.getX(), p2.getY(),  p3.getX(), p3.getY(),r0.getX(), r0.getY())==1)return null;
	   if (Line2D.relativeCCW( p3.getX(), p3.getY(),  p1.getX(), p1.getY(),r0.getX(), r0.getY())==1)return null;

	    return i;                      // I is in T
	}
	
	
	public Vec3D rayPlaneIntersection(HasPos3D r0,HasPos3D r1,boolean seg){
		 HasPos3D dir=Vec3D.sub(r1,r0);
		 HasPos3D  w0=Vec3D.sub(r0, getNode(0));
		   
		 double   a = -1f*Vec3D.dot3D(n,w0);
		 double    b = Vec3D.dot3D(n,dir);
		    if (Math.abs(b) < SMALL_NUM) {     // ray is parallel to triangle plane
		        if (a == 0)                // ray lies in triangle plane
		            return null;
		        else return null;             // ray disjoint from plane
		    }

		    // get intersect point of ray with triangle plane
		    double  r = a / b;
		    if (r < 0.0)                   // ray goes away from triangle
		        return null;                  // => no intersect
		    if (seg){
		    	if (r>1.0)return null;
		    }
		    // for a segment, also test if (r > 1.0) => no intersect
		  return Vec3D.add(r0, Vec3D.mult(dir,(float) r));
	}
	
	public float distanceToPlane(float x, float y, float z){
			return n.dot(x, y, z)+d0;
	}
	
	public float getDistanceSqr(float x, float y, float z) {
		PMatrix3D m = new PMatrix3D();
		Vec3D planeAxis = new Vec3D(0, 0, 1);
		Line3D l1;
		Line3D l2;
		Line3D l3;

		HasPos3D v1 = getNode(0);
		HasPos3D v2 = getNode(1);
		HasPos3D v3 = getNode(2);
		Vec3D v12 = Vec3D.sub(v2, v1);
		Vec3D v13 = Vec3D.sub(v3, v1);
		Vec3D n = Vec3D.cross(v12, v13);
		n.normalize();
		float angle = (float) Math.acos(n.dot(planeAxis));

		Vec3D u = n.cross(planeAxis);
		u.normalize();

		float c = (float) Math.cos(angle);
		float s = (float) Math.sin(angle);
		float t = 1f - c;

		m.m00 = c + u.getX() * u.getX() * t;
		m.m01 = u.getX() * u.getY() * t - u.getZ() * s;
		m.m02 = u.getX() * u.getZ() * t + u.getY() * s;

		m.m10 = u.getY() * u.getX() * t + u.getZ() * s;
		m.m11 = c + u.getY() * u.getY() * t;
		m.m12 = u.getY() * u.getZ() * t - u.getX() * s;

		m.m20 = u.getZ() * u.getX() * t - u.getY() * s;
		m.m21 = u.getZ() * u.getY() * t + u.getX() * s;
		m.m22 = c + u.getZ() * u.getZ() * t;

		PVector[] pts = new PVector[3];
		float[] f1 = new float[] { v1.getX(), v1.getY(), v1.getZ() };
		float[] f2 = new float[] { v2.getX(), v2.getY(), v2.getZ() };
		float[] f3 = new float[] { v3.getX(), v3.getY(), v3.getZ() };

		float[] f1T = m.mult(f1, null);
		float[] f2T = m.mult(f2, null);
		float[] f3T = m.mult(f3, null);

		l1 = new Line3D(f1T[0], f1T[1], f1T[2], f2T[0], f2T[1], f2T[2]);
		l2 = new Line3D(f2T[0], f2T[1], f2T[2], f3T[0], f3T[1], f3T[2]);
		l3 = new Line3D(f3T[0], f3T[1], f3T[2], f1T[0], f1T[1], f1T[2]);

		float minD = Float.MAX_VALUE;
		float[] pt = new float[] { x, y, z };
		float[] trans = new float[3];

		trans = m.mult(pt, null);
		float cX = trans[0];
		float cY = trans[1];
		float cZ = trans[2];
	

		int r1 = Line2D.relativeCCW(l1.x1, l1.y1, l1.x2, l1.y2, cX, cY);
		int r2 = Line2D.relativeCCW(l2.x1, l2.y1, l2.x2, l2.y2, cX, cY);
		int r3 = Line2D.relativeCCW(l3.x1, l3.y1, l3.x2, l3.y2, cX, cY);
		float cDistance;
		if (r1 == r2 && r1 == r3) {
			cDistance = (cZ - l1.z1) * (cZ - l1.z1);
			if (cDistance < minD) {
				minD = cDistance;
			}
		} else {
			float d1 = (float) l1.ptSegDistSq(cX, cY, cZ);
			float d2 = (float) l2.ptSegDistSq(cX, cY, cZ);
			float d3 = (float) l3.ptSegDistSq(cX, cY, cZ);
			if (d1 < minD) {
				minD = d1;
			}
			if (d2 < minD) {
				minD = d2;
			}
			if (d3 < minD) {
				minD = d3;
			}
		}

		return (float) minD;

	}
	public float getDistance(float x, float y, float z) {
		return (float) Math.sqrt(getDistanceSqr(x,y,z));
	}
	
}
